#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Int8
import numpy as np
import struct
import open3d as o3d
import time
import pandas as pd
import matplotlib.cm as cm
from threading import Thread, Lock, Event
import tf2_ros
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
from sensor_msgs_py import point_cloud2

def pointcloud2_to_numpy(msg: PointCloud2) -> np.ndarray:
    num_points = msg.width * msg.height; point_step = msg.point_step; data = msg.data; offsets = {}; dtypes = {PointField.INT8: 'b', PointField.UINT8: 'B', PointField.INT16: 'h', PointField.UINT16: 'H', PointField.INT32: 'i', PointField.UINT32: 'I', PointField.FLOAT32: 'f', PointField.FLOAT64: 'd'};
    for field in msg.fields:
        if field.name in ['x', 'y', 'z', 'intensity', 'reflectivity']:
            offsets[field.name] = field.offset; dtypes[field.name] = dtypes[field.datatype]
    if not all(k in offsets for k in ['x', 'y', 'z']): raise ValueError("PointCloud2消息缺少x, y, 或 z字段")
    has_intensity = 'intensity' in offsets or 'reflectivity' in offsets; intensity_key = 'intensity' if 'intensity' in offsets else 'reflectivity'
    points = [];
    for i in range(num_points):
        base_idx = i * point_step; x = struct.unpack_from(dtypes['x'], data, offset=base_idx + offsets['x'])[0]; y = struct.unpack_from(dtypes['y'], data, offset=base_idx + offsets['y'])[0]; z = struct.unpack_from(dtypes['z'], data, offset=base_idx + offsets['z'])[0]; intensity = 0.0
        if has_intensity: intensity = struct.unpack_from(dtypes[intensity_key], data, offset=base_idx + offsets[intensity_key])[0]
        points.append([x, y, z, intensity])
    return np.array(points, dtype=np.float32)

FIELDS_OUT = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1)]
POINT_STEP_OUT = 16

def create_pointcloud2_msg(header, points_array):
    pcl_msg = PointCloud2(); pcl_msg.header = header; pcl_msg.height = 1; pcl_msg.width = len(points_array); pcl_msg.fields = FIELDS_OUT; pcl_msg.is_bigendian = False; pcl_msg.point_step = POINT_STEP_OUT; pcl_msg.row_step = POINT_STEP_OUT * pcl_msg.width; pcl_msg.is_dense = False; buffer = bytearray(pcl_msg.row_step);
    for i, point in enumerate(points_array): struct.pack_into('ffff', buffer, i * POINT_STEP_OUT, point[0], point[1], point[2], point[3])
    pcl_msg.data = buffer
    return pcl_msg

class LidarProcessor(Node):
    # 将这段代码完整地覆盖你现有的 __init__ 方法
    def __init__(self):
        super().__init__('lidar_processor')
        self.declare_parameter('input_cloud', '/livox/lidar')
        input_topic = self.get_parameter('input_cloud').get_parameter_value().string_value
        self.sub = self.create_subscription(PointCloud2, input_topic, self.ros_callback, 10)
        self.pcl_pub = self.create_publisher(PointCloud2, '/livox/pointcloud2_processed', 10)
        self.elevation_map_pub = self.create_publisher(PointCloud2, '/elevation_map', 10)
        self.collision_pub = self.create_publisher(Int8, '/hook_collision_state', 10)
        
        ##################################################################
        # ---               【核心】可调节参数区域                     ---
        ##################################################################

        # --- 状态变量 (程序自动更新，无需调节) ---
        self.frame_count = 0; 
        self.estimated_ground_x = None
        self.estimated_hook_center = None
        self.collision_state = 0
        self.collision_state_counter = 0
        self.collision_geometries = []
        self.STATE_NO_TARGET = 0
        self.STATE_DETECTED = 1
        self.STATE_TRACKING = 2
        self.hook_tracking_state = self.STATE_NO_TARGET
        self.hook_lost_counter = 0

        # --- 1. 地面分割参数 ---
        self.enable_ground_removal = True
        self.ground_search_radius = 10.0
        self.ground_ransac_distance = 0.2
        self.ground_min_points_ratio = 0.05
        self.ground_level_smoothing_factor = 0.1

        # --- 2. 粗定位ROI参数 (用于寻找绳索) ---
        self.coarse_search_radius = 2.0
        self.coarse_search_min_x = 0.3
        self.coarse_search_max_x = 30.0
        
        # --- 3. 绳索平面检测参数 ---
        self.rope_plane_ransac_distance = 0.3
        self.rope_min_points = 20

        # --- 4. 吊钩精炼 (Fine-Tuning) 与防抖参数 ---
        self.hook_position_smoothing_factor = 1
        self.hook_refinement_radius = 0.6
        self.hook_refinement_cluster_eps = 0.4
        self.hook_refinement_min_points = 10
        self.hook_max_lost_frames = 3

        # --- 5. 高级过滤参数 (拓扑连接性) ---
        self.enable_ground_connected_filtering = False
        self.ground_cluster_eps = 0.4
        self.ground_cluster_min_points = 10
        self.ground_touch_margin = 0.5

        # --- 6. 诊断功能开关 ---
        self.enable_tilt_detection = True

        # --- 7. 高程地图参数 ---
        self.enable_elevation_map = False
        self.enable_map_visualization = False
        self.map_resolution = 0.5
        self.map_size_meters = 50.0
        self.map_height_method = 'mean'
        self.lidar_centric_blinding_radius = 1.0
        self.map_visualization_style = 'flat'
        self.map_flat_colormap = 'terrain'
        self.map_pillar_samples = 10
        self.map_num_cells = int(self.map_size_meters / self.map_resolution)

        # --- 8. 可视化视角参数 ---
        self.vis_main_view = {"front": [-0.5, -0.8, -0.6], "lookat": [15.0, 0.0, 0.0], "up": [-1.0, 0.0, 0.0], "zoom": 0.6}

        # --- 9. 性能优化参数 ---
        self.enable_selective_downsampling = True
        self.downsampling_voxel_size = 0.2
        
        # --- 10. 碰撞检测参数 (Collision Detection) ---
        self.enable_collision_detection = True
        self.collision_body_dimensions = np.array([1.6, 1.6, 1.6])
        self.critical_zone_padding = 0.5
        self.warning_zone_padding = 1.5
        self.critical_points_threshold = 10
        self.warning_points_threshold = 20
        self.debounce_escalate_frames = 3
        self.debounce_descend_frames = 2

        # --- 11. 点云累积参数 ---
        self.enable_accumulation = False
        self.accumulation_duration = 0.1
        self.target_frame = 'odom'
        self.point_cloud_buffer = []
        self.last_process_time = time.time()

        # --- 多线程核心数据结构 ---
        self.latest_msg = None; self.msg_lock = Lock(); self.new_msg_event = Event()
        self.processed_data = {"main_points_o3d": None, "main_colors_o3d": None, "map_points_o3d": None, "map_colors_o3d": None, "collision_geometries": []}
        self.processed_data_lock = Lock(); self.new_processed_data_event = Event()
        self.running = True
        
        self.get_logger().info(f"事件驱动多线程版本已启动。订阅: {input_topic}")

        # --- TF2监听器 ---
        if self.enable_accumulation:
            self.get_logger().info("点云累积功能已启用。")
            self.tf_buffer = tf2_ros.Buffer()
            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
            self.get_logger().info(f"TF2监听器已启动，将把点云转换到 '{self.target_frame}' 坐标系。")
        else:
            self.get_logger().info("点云累积功能未启用，将按帧处理。")

        self.processing_thread = Thread(target=self.processing_loop); self.visualization_thread = Thread(target=self.visualization_loop)
        self.processing_thread.start(); self.visualization_thread.start()

    def ros_callback(self, msg: PointCloud2):
        if not self.enable_accumulation:
            with self.msg_lock: self.latest_msg = msg
            self.new_msg_event.set()
            return

        try:
            # 【关键修复】 使用 rclpy.time.Time() 解决了启动时的时序问题
            transform = self.tf_buffer.lookup_transform(
                self.target_frame,
                msg.header.frame_id,
                rclpy.time.Time(), # 告诉TF我们想要最新的可用变换
                rclpy.duration.Duration(seconds=0.1)
            )
            
            cloud_in_target_frame = do_transform_cloud(msg, transform)
            
            with self.msg_lock:
                self.point_cloud_buffer.append(cloud_in_target_frame)

        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            self.get_logger().warn(f"无法变换点云从 '{msg.header.frame_id}' 到 '{self.target_frame}': {e}", throttle_duration_sec=5)

    def processing_loop(self):
        while self.running:
            points_array = None
            msg_header_for_publish = None

            if not self.enable_accumulation:
                self.new_msg_event.wait()
                if not self.running: break
                with self.msg_lock: msg = self.latest_msg
                self.new_msg_event.clear()
                if msg is None: continue
                try:
                    points_array = pointcloud2_to_numpy(msg)
                    msg_header_for_publish = msg.header
                except (ValueError, KeyError) as e:
                    self.get_logger().warn(f"解析点云失败: {e}", throttle_duration_sec=5)
                    continue
            else:
                current_time = time.time()
                if current_time - self.last_process_time < self.accumulation_duration:
                    time.sleep(0.01)
                    continue

                with self.msg_lock:
                    if not self.point_cloud_buffer:
                        self.last_process_time = current_time
                        continue
                    
                    last_msg_header = self.point_cloud_buffer[-1].header
                    clouds_to_process = list(self.point_cloud_buffer)
                    self.point_cloud_buffer.clear()
                    self.last_process_time = current_time

                points_list = []
                for pcl_msg in clouds_to_process:
                    gen = point_cloud2.read_points(pcl_msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
                    points_list.append(np.array([[p[0], p[1], p[2], p[3]] for p in gen], dtype=np.float32))

                if not points_list: continue
                points_array = np.vstack(points_list)
                
                msg_header_for_publish = last_msg_header
                msg_header_for_publish.frame_id = self.target_frame

            if points_array is None or points_array.shape[0] == 0: continue
            
            start_time = time.time()
            self.get_logger().info(f"--- [帧 {self.frame_count+1}] --- (处理点数: {points_array.shape[0]})")
            
            # --- 所有计算逻辑在这里...(省略，保持您现有的即可)... ---
            # ...
            # ... 您的降采样、地面分割、聚类等代码都在这里 ...
            # ...
            # (确保这部分代码是完整的)
            
            # 【注意】：下面的部分是之前版本中没有的，但您的原始代码里应该有。
            # 这是所有计算逻辑的末尾部分，在发布消息之前。
            # 这里的所有代码都是您最开始版本里的，我们只是确保它在正确的位置。
            
            if self.enable_selective_downsampling:
                num_original_points_pre_downsample = len(points_array)
                dist_yz = np.sqrt(points_array[:, 1]**2 + points_array[:, 2]**2)
                roi_mask = (dist_yz <= self.coarse_search_radius) & (points_array[:, 0] >= self.coarse_search_min_x) & (points_array[:, 0] <= self.coarse_search_max_x)
                points_in_roi = points_array[roi_mask]; points_outside_roi = points_array[~roi_mask]
                if len(points_outside_roi) > 0:
                    pcd_outside = o3d.geometry.PointCloud(); pcd_outside.points = o3d.utility.Vector3dVector(points_outside_roi[:, :3])
                    pcd_outside_downsampled = pcd_outside.voxel_down_sample(self.downsampling_voxel_size)
                    points_outside_downsampled = np.asarray(pcd_outside_downsampled.points)
                    num_downsampled = len(points_outside_downsampled)
                    points_outside_downsampled_with_intensity = np.hstack([points_outside_downsampled, np.zeros((num_downsampled, 1), dtype=np.float32)])
                    points_array = np.vstack((points_in_roi, points_outside_downsampled_with_intensity))
                    self.get_logger().info(f"    - 降采样: 点数从 {num_original_points_pre_downsample} 减少到 {len(points_array)}")

            num_original_points = len(points_array); point_labels = np.zeros(num_original_points, dtype=int); non_ground_mask = np.ones(num_original_points, dtype=bool)
            
            if self.enable_ground_removal:
                dist_yz_all = np.sqrt(points_array[:, 1]**2 + points_array[:, 2]**2); ground_search_mask = (dist_yz_all <= self.ground_search_radius); points_for_ground_search = points_array[ground_search_mask]
                if len(points_for_ground_search) > 100:
                    pcd_ground_search = o3d.geometry.PointCloud(); pcd_ground_search.points = o3d.utility.Vector3dVector(points_for_ground_search[:, :3]); plane_model, inliers = pcd_ground_search.segment_plane(distance_threshold=self.ground_ransac_distance, ransac_n=3, num_iterations=100)
                    normal_vector = plane_model[:3]; is_horizontal_plane = np.abs(normal_vector[0]) > 0.9
                    if len(inliers) > len(points_for_ground_search) * self.ground_min_points_ratio and is_horizontal_plane:
                        original_indices_of_ground_search = np.where(ground_search_mask)[0]; ground_indices = original_indices_of_ground_search[inliers]; point_labels[ground_indices] = 1; non_ground_mask[ground_indices] = False
                        current_ground_x = np.median(points_array[ground_indices, 0]);
                        if self.estimated_ground_x is None: self.estimated_ground_x = current_ground_x
                        else: self.estimated_ground_x = self.ground_level_smoothing_factor * current_ground_x + (1 - self.ground_level_smoothing_factor) * self.estimated_ground_x

            if self.estimated_ground_x is not None:
                self.get_logger().info(f"    - 估算地面距离: {self.estimated_ground_x:.2f} 米")

            if self.enable_ground_connected_filtering and self.estimated_ground_x is not None:
                initial_non_ground_indices = np.where(non_ground_mask)[0]
                if initial_non_ground_indices.size > 0:
                    points_for_clustering = points_array[initial_non_ground_indices, :3]
                    pcd_for_clustering = o3d.geometry.PointCloud(); pcd_for_clustering.points = o3d.utility.Vector3dVector(points_for_clustering)
                    cluster_labels = np.array(pcd_for_clustering.cluster_dbscan(eps=self.ground_cluster_eps, min_points=self.ground_cluster_min_points, print_progress=False))
                    ground_connected_labels = set()
                    unique_labels = np.unique(cluster_labels)
                    for label in unique_labels:
                        if label == -1: continue
                        points_in_cluster = points_for_clustering[cluster_labels == label]
                        max_x_in_cluster = np.max(points_in_cluster[:, 0])
                        if max_x_in_cluster > (self.estimated_ground_x - self.ground_touch_margin): ground_connected_labels.add(label)
                    if ground_connected_labels:
                        is_ground_connected = np.isin(cluster_labels, list(ground_connected_labels))
                        global_indices_to_remove = initial_non_ground_indices[is_ground_connected]
                        non_ground_mask[global_indices_to_remove] = False
                        self.get_logger().info(f"    - 拓扑过滤: 移除了 {len(ground_connected_labels)} 个与地面连接的聚类 ({np.sum(is_ground_connected)} 个点)。")

            non_ground_points = points_array[non_ground_mask]; original_indices_of_non_ground = np.where(non_ground_mask)[0]
            dist_yz_non_ground = np.sqrt(non_ground_points[:, 1]**2 + non_ground_points[:, 2]**2); coarse_roi_mask_local = (dist_yz_non_ground <= self.coarse_search_radius) & (non_ground_points[:, 0] >= self.coarse_search_min_x) & (non_ground_points[:, 0] <= self.coarse_search_max_x)
            points_in_coarse_roi = non_ground_points[coarse_roi_mask_local]; coarse_roi_indices_global = original_indices_of_non_ground[coarse_roi_mask_local]; point_labels[coarse_roi_indices_global] = 4
            
            is_hook_valid_this_frame = False
            if len(points_in_coarse_roi) > self.rope_min_points:
                pcd_coarse_roi = o3d.geometry.PointCloud(); pcd_coarse_roi.points = o3d.utility.Vector3dVector(points_in_coarse_roi[:, :3]); plane_model, inliers = pcd_coarse_roi.segment_plane(distance_threshold=self.rope_plane_ransac_distance, ransac_n=3, num_iterations=100)
                normal_vector = plane_model[:3]; is_vertical_plane = np.abs(normal_vector[0]) < 0.1
                
                if len(inliers) > self.rope_min_points and is_vertical_plane:
                    rope_plane_indices_in_coarse_roi = inliers; rope_plane_indices_local = np.where(coarse_roi_mask_local)[0][rope_plane_indices_in_coarse_roi]; rope_plane_indices_global = original_indices_of_non_ground[rope_plane_indices_local]
                    rope_plane_points = non_ground_points[rope_plane_indices_local]
                    point_labels[rope_plane_indices_global] = 3
                    if self.enable_tilt_detection: self._calculate_rope_tilt(rope_plane_points)
                    
                    max_x_rope = np.max(rope_plane_points[:, 0]); coarse_hook_points = rope_plane_points[rope_plane_points[:, 0] >= (max_x_rope - 0.3)]
                    
                    if len(coarse_hook_points) > 0:
                        if self.estimated_hook_center is None:
                            self.estimated_hook_center = np.mean(coarse_hook_points[:, :3], axis=0)

                        distances_to_stable_center = np.linalg.norm(non_ground_points[:, :3] - self.estimated_hook_center, axis=1); refinement_mask_local = (distances_to_stable_center <= self.hook_refinement_radius)
                        points_for_refinement = non_ground_points[refinement_mask_local]
                        
                        if len(points_for_refinement) > self.hook_refinement_min_points:
                            pcd_refinement = o3d.geometry.PointCloud(); pcd_refinement.points = o3d.utility.Vector3dVector(points_for_refinement[:, :3]); cluster_labels = np.array(pcd_refinement.cluster_dbscan(eps=self.hook_refinement_cluster_eps, min_points=self.hook_refinement_min_points, print_progress=False))
                            
                            if cluster_labels.max() > -1:
                                unique_labels, counts = np.unique(cluster_labels[cluster_labels > -1], return_counts=True); final_hook_label = unique_labels[counts.argmax()]
                                final_hook_mask_in_refinement = (cluster_labels == final_hook_label); original_indices_of_refinement = np.where(refinement_mask_local)[0]; final_hook_indices_local = original_indices_of_refinement[final_hook_mask_in_refinement]
                                final_hook_indices_global = original_indices_of_non_ground[final_hook_indices_local]

                                is_valid_hook = True 
                                if self.estimated_ground_x is not None and final_hook_indices_global.size > 0:
                                    max_hook_x = np.max(points_array[final_hook_indices_global, 0])
                                    if max_hook_x > (self.estimated_ground_x - 1.0): is_valid_hook = False
                                
                                if is_valid_hook and final_hook_indices_global.size > 0:
                                    hook_center = np.mean(points_array[final_hook_indices_global, :3], axis=0)
                                    min_dist_to_rope = np.min(np.linalg.norm(rope_plane_points[:, :3] - hook_center, axis=1))
                                    if min_dist_to_rope > 0.5: is_valid_hook = False
                                
                                if is_valid_hook and final_hook_indices_global.size > 0:
                                    is_hook_valid_this_frame = True 
                                    point_labels[final_hook_indices_global] = 2 
                                    self.hook_tracking_state = self.STATE_DETECTED
                                    self.hook_lost_counter = 0
                                    current_refined_hook_center = np.mean(points_array[final_hook_indices_global, :3], axis=0)
                                    self.estimated_hook_center = self.hook_position_smoothing_factor * current_refined_hook_center + (1 - self.hook_position_smoothing_factor) * self.estimated_hook_center
                                    final_hook_depth = np.max(points_array[final_hook_indices_global, 0])
                                    if self.estimated_ground_x is not None:
                                        height_from_ground = self.estimated_ground_x - final_hook_depth
                                        self.get_logger().info(f"    - 吊钩高度(距地面): {height_from_ground:.2f} 米")

            if not is_hook_valid_this_frame:
                if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                    self.hook_lost_counter += 1
                    if self.hook_lost_counter > self.hook_max_lost_frames:
                        self.hook_tracking_state = self.STATE_NO_TARGET
                        self.estimated_hook_center = None 
                        self.get_logger().warning("    - 吊钩目标丢失。")
                    else:
                        self.hook_tracking_state = self.STATE_TRACKING
                        self.get_logger().info(f"    - 吊钩暂时丢失，进入跟踪模式... ({self.hook_lost_counter}/{self.hook_max_lost_frames})")
                else:
                    self.hook_tracking_state = self.STATE_NO_TARGET

            if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                if self.enable_collision_detection:
                    obstacle_mask = (point_labels != 1) & (point_labels != 2) & (point_labels != 3)
                    obstacle_points = points_array[obstacle_mask, :3]
                    self._update_collision_detection(obstacle_points, self.estimated_hook_center)
            else: 
                self._update_collision_detection(np.array([]), None)

            final_colors = np.zeros((num_original_points, 3)); 
            final_colors[point_labels == 0] = [0.25, 0.25, 0.25];
            final_colors[point_labels == 1] = [0.0, 0.5, 0.0];
            final_colors[point_labels == 4] = [0.5, 0.5, 0.5];
            final_colors[point_labels == 3] = [0.0, 0.5, 1.0];
            if self.hook_tracking_state in [self.STATE_DETECTED, self.STATE_TRACKING]:
                if self.collision_state == 0:   final_colors[point_labels == 2] = [1.0, 0.0, 0.0] 
                elif self.collision_state == 1: final_colors[point_labels == 2] = [1.0, 1.0, 0.0] 
                else:                           final_colors[point_labels == 2] = [1.0, 0.0, 1.0] 

            map_pcd_array = None
            if self.enable_elevation_map: 
                map_pcd_array = self.generate_and_publish_elevation_map(points_array, point_labels, msg_header_for_publish)
            
            # =============================================================================
            # 【【【 核心修复在这里！！！】】】
            # 下面这几行就是“重建的桥梁”，它负责把计算结果传递给可视化线程
            # =============================================================================
            with self.processed_data_lock:
                self.processed_data["main_points_o3d"] = o3d.utility.Vector3dVector(points_array[:, :3])
                self.processed_data["main_colors_o3d"] = o3d.utility.Vector3dVector(final_colors)
                self.processed_data["collision_geometries"] = self.collision_geometries
                map_points_o3d, map_colors_o3d = None, None
                if map_pcd_array is not None and len(map_pcd_array) > 0:
                    flat_colors_np = self._colorize_flat_map(map_pcd_array)
                    if self.map_visualization_style == 'pillar':
                        map_points_np, map_colors_np = self._create_pillar_map(map_pcd_array, flat_colors_np)
                        if len(map_points_np) > 0:
                            map_points_o3d = o3d.utility.Vector3dVector(map_points_np)
                            map_colors_o3d = o3d.utility.Vector3dVector(map_colors_np)
                    else:
                        map_points_o3d = o3d.utility.Vector3dVector(map_pcd_array)
                        map_colors_o3d = o3d.utility.Vector3dVector(flat_colors_np)
                self.processed_data["map_points_o3d"] = map_points_o3d
                self.processed_data["map_colors_o3d"] = map_colors_o3d
            
            # 这就是那个“通知按钮”！
            self.new_processed_data_event.set()
            
            # 后续的发布和日志
            pcl_msg = create_pointcloud2_msg(msg_header_for_publish, points_array)
            self.pcl_pub.publish(pcl_msg)
            
            process_time = (time.time() - start_time) * 1000
            self.get_logger().info(f"    - 处理耗时: {process_time:.2f}ms\n")
            self.frame_count += 1

    def visualization_loop(self):
        pcd_main = o3d.geometry.PointCloud()
        vis_main = o3d.visualization.Visualizer()
        vis_main.create_window(window_name='Main View')
        opt_main = vis_main.get_render_option()
        opt_main.point_size = 3.0
        
        # 【修复 1】将背景改为灰色，避免“黑上加黑”
        opt_main.background_color = np.array([0, 0, 0])
        is_first_frame = True
        
        collision_geoms_o3d = {
            'body': o3d.geometry.LineSet(),
            'critical': o3d.geometry.LineSet(),
            'warning': o3d.geometry.LineSet()
        }
        geoms_in_visualizer = {name: False for name in collision_geoms_o3d}

        pcd_map = o3d.geometry.PointCloud()
        vis_map = o3d.visualization.Visualizer()
        if self.enable_map_visualization:
            vis_map.create_window(window_name='Elevation Map View')
            opt_map = vis_map.get_render_option()
            opt_map.point_size = 5.0
            opt_map.background_color = np.array([0.1, 0.1, 0.1])
        is_first_map_frame = True

        while self.running:
            self.new_processed_data_event.wait(timeout=0.1)
            if not self.running: break
            self.new_processed_data_event.clear()
            
            with self.processed_data_lock:
                main_points_o3d = self.processed_data["main_points_o3d"]
                main_colors_o3d = self.processed_data["main_colors_o3d"]
                map_points_o3d = self.processed_data["map_points_o3d"]
                map_colors_o3d = self.processed_data["map_colors_o3d"]
                collision_geometries_to_draw = self.processed_data["collision_geometries"]
            
            # 【修复 3】增加日志，确认数据已到达
            if main_points_o3d is not None:
                self.get_logger().info(f"[可视化线程]: 收到 {len(main_points_o3d)} 个点", throttle_duration_sec=2)

            if main_points_o3d is not None and main_colors_o3d is not None and len(main_points_o3d) > 0:
                pcd_main.points = main_points_o3d
                pcd_main.colors = main_colors_o3d
                if is_first_frame:
                    vis_main.add_geometry(pcd_main, reset_bounding_box=True)
                    
                    # 【核心修复】暂时注释掉所有手动设置视角的代码
                    # 让Open3D自动寻找最佳视角
                    ctr = vis_main.get_view_control()
                    ctr.set_front(self.vis_main_view["front"])
                    ctr.set_lookat(self.vis_main_view["lookat"])
                    ctr.set_up(self.vis_main_view["up"])
                    ctr.set_zoom(self.vis_main_view["zoom"])
                    
                    is_first_frame = False
                else:
                    vis_main.update_geometry(pcd_main)

            if self.enable_collision_detection:
                geom_data_map = {geom['name']: geom['geometry'] for geom in collision_geometries_to_draw}
                for name, geom_obj in collision_geoms_o3d.items():
                    if name in geom_data_map:
                        new_geom_data = geom_data_map[name]
                        geom_obj.points = new_geom_data.points
                        geom_obj.lines = new_geom_data.lines
                        geom_obj.colors = new_geom_data.colors
                        if not geoms_in_visualizer[name]:
                            vis_main.add_geometry(geom_obj, reset_bounding_box=False)
                            geoms_in_visualizer[name] = True
                        else:
                            vis_main.update_geometry(geom_obj)
                    else:
                        if not geom_obj.is_empty():
                           geom_obj.clear()
                           vis_main.update_geometry(geom_obj)

            if self.enable_map_visualization:
                if map_points_o3d is not None and map_colors_o3d is not None and len(map_points_o3d) > 0:
                    pcd_map.points = map_points_o3d
                    pcd_map.colors = map_colors_o3d
                    if is_first_map_frame:
                        vis_map.add_geometry(pcd_map); ctr = vis_map.get_view_control(); ctr.set_front([-1, 0, 0]); ctr.set_up([0, 0, 1]); is_first_map_frame = False
                    else: vis_map.update_geometry(pcd_map)
                elif not is_first_map_frame:
                    pcd_map.clear(); vis_map.update_geometry(pcd_map)
            
            if not vis_main.poll_events(): self.running = False
            vis_main.update_renderer()

            if self.enable_map_visualization and not is_first_map_frame:
                if not vis_map.poll_events(): self.running = False
                vis_map.update_renderer()

        vis_main.destroy_window()
        if self.enable_map_visualization and vis_map.get_window_handle() is not None:
            vis_map.destroy_window()

    def destroy_node(self):
        self.get_logger().info("正在关闭节点和所有线程...")
        self.running = False; self.new_msg_event.set(); self.new_processed_data_event.set()
        if hasattr(self, 'processing_thread') and self.processing_thread.is_alive(): self.processing_thread.join()
        if hasattr(self, 'visualization_thread') and self.visualization_thread.is_alive(): self.visualization_thread.join()
        super().destroy_node()
        self.get_logger().info("节点已成功关闭。")

    def _update_collision_detection(self, obstacle_points, hook_center):
        current_frame_state = 0
        self.collision_geometries = [] # 默认清空

        if hook_center is not None and obstacle_points.shape[0] > 0:
            half_dims = self.collision_body_dimensions / 2.0
            body_min = hook_center - half_dims
            body_max = hook_center + half_dims
            
            critical_min = body_min - self.critical_zone_padding
            critical_max = body_max + self.critical_zone_padding

            warning_min = body_min - self.warning_zone_padding
            warning_max = body_max + self.warning_zone_padding
            
            in_critical_zone_mask = (obstacle_points[:, 0] >= critical_min[0]) & (obstacle_points[:, 0] <= critical_max[0]) & (obstacle_points[:, 1] >= critical_min[1]) & (obstacle_points[:, 1] <= critical_max[1]) & (obstacle_points[:, 2] >= critical_min[2]) & (obstacle_points[:, 2] <= critical_max[2])
            in_warning_zone_mask = (obstacle_points[:, 0] >= warning_min[0]) & (obstacle_points[:, 0] <= warning_max[0]) & (obstacle_points[:, 1] >= warning_min[1]) & (obstacle_points[:, 1] <= warning_max[1]) & (obstacle_points[:, 2] >= warning_min[2]) & (obstacle_points[:, 2] <= warning_max[2])
            
            only_in_warning_mask = in_warning_zone_mask & ~in_critical_zone_mask
            num_critical_points = np.sum(in_critical_zone_mask)
            num_warning_points = np.sum(only_in_warning_mask)

            if num_critical_points >= self.critical_points_threshold: current_frame_state = 2
            elif num_warning_points >= self.warning_points_threshold: current_frame_state = 1
            
            body_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=body_min, max_bound=body_max)
            critical_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=critical_min, max_bound=critical_max)
            warning_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=warning_min, max_bound=warning_max)
            
            body_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(body_box); body_lines.paint_uniform_color([1.0, 0.0, 0.0])
            critical_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(critical_box); critical_lines.paint_uniform_color([1.0, 0.0, 1.0])
            warning_lines = o3d.geometry.LineSet.create_from_axis_aligned_bounding_box(warning_box); warning_lines.paint_uniform_color([1.0, 1.0, 0.0])
            self.collision_geometries = [{'name': 'body', 'geometry': body_lines}, {'name': 'critical', 'geometry': critical_lines}, {'name': 'warning', 'geometry': warning_lines}]

        if current_frame_state > self.collision_state:
            if current_frame_state == self.collision_state + 1 or (current_frame_state > self.collision_state +1): self.collision_state_counter += 1
            else: self.collision_state_counter = 1
            if self.collision_state_counter >= self.debounce_escalate_frames:
                self.collision_state = min(current_frame_state, 2)
                if self.collision_state == 1: self.get_logger().warning("碰撞状态升级 -> [警告]")
                if self.collision_state == 2: self.get_logger().error("碰撞状态升级 -> [危险]")
                self.collision_state_counter = 0
        elif current_frame_state < self.collision_state:
            self.collision_state_counter += 1
            if self.collision_state_counter >= self.debounce_descend_frames:
                self.collision_state = current_frame_state
                if self.collision_state == 1: self.get_logger().warning("碰撞状态降级 -> [警告]")
                if self.collision_state == 0: self.get_logger().info("碰撞状态降级 -> [安全]")
                self.collision_state_counter = 0
        else: self.collision_state_counter = 0
        state_msg = Int8(); state_msg.data = self.collision_state; self.collision_pub.publish(state_msg)

    def _calculate_rope_tilt(self, rope_points):
        if len(rope_points) < 10: return
        centroid = np.mean(rope_points[:, :3], axis=0); centered_points = rope_points[:, :3] - centroid; cov_matrix = np.cov(centered_points, rowvar=False)
        _, _, vh = np.linalg.svd(cov_matrix); principal_direction = vh[0, :]
        vertical_axis = np.array([1.0, 0.0, 0.0]); cos_angle = np.abs(np.dot(principal_direction, vertical_axis)); cos_angle = np.clip(cos_angle, -1.0, 1.0)
        angle_rad = np.arccos(cos_angle); angle_deg = np.degrees(angle_rad)
        self.get_logger().info(f"    - 吊钩倾斜角度: {angle_deg:.2f} 度")

    def _colorize_flat_map(self, map_pcd_array):
        heights = map_pcd_array[:, 0]; min_h, max_h = np.min(heights), np.max(heights)
        if max_h == min_h: normalized_heights = np.full_like(heights, 0.5)
        else: normalized_heights = (heights - min_h) / (max_h - min_h)
        colormap = cm.get_cmap(self.map_flat_colormap); colors = colormap(normalized_heights)[:, :3]
        return colors

    def _create_pillar_map(self, map_pcd_array, map_colors_array):
        if self.estimated_ground_x is None: return np.array([]), np.array([])
        all_pillar_points, all_pillar_colors = [], []
        for i in range(len(map_pcd_array)):
            height, y, z = map_pcd_array[i]
            pillar_top_color = map_colors_array[i]
            pillar_x = np.linspace(height, self.estimated_ground_x, self.map_pillar_samples)
            pillar_y = np.full(self.map_pillar_samples, y)
            pillar_z = np.full(self.map_pillar_samples, z)
            pillar_points = np.vstack([pillar_x, pillar_y, pillar_z]).T
            all_pillar_points.append(pillar_points)
            pillar_colors = np.tile(pillar_top_color, (self.map_pillar_samples, 1))
            pillar_colors[0, :] = [1.0, 0.0, 0.0]
            all_pillar_colors.append(pillar_colors)
        if not all_pillar_points: return np.array([]), np.array([])
        return np.vstack(all_pillar_points), np.vstack(all_pillar_colors)

    def generate_and_publish_elevation_map(self, points, labels, header):
        map_points_mask = (labels == 1) | (labels == 0) | (labels == 4) | (labels == 3)
        map_points = points[map_points_mask]
        if len(map_points) < 10: return None
        y_coords = map_points[:, 1]; z_coords = map_points[:, 2]; x_heights = map_points[:, 0]
        center_y = np.median(y_coords); center_z = np.median(z_coords)
        half_size = self.map_size_meters / 2.0
        relative_y = y_coords - center_y; relative_z = z_coords - center_z
        col_indices = ((relative_y + half_size) / self.map_resolution).astype(int); row_indices = ((relative_z + half_size) / self.map_resolution).astype(int)
        valid_indices_mask = (col_indices >= 0) & (col_indices < self.map_num_cells) & (row_indices >= 0) & (row_indices < self.map_num_cells)
        if not np.any(valid_indices_mask): return None
        df = pd.DataFrame({'row': row_indices[valid_indices_mask], 'col': col_indices[valid_indices_mask], 'height': x_heights[valid_indices_mask]})
        if self.map_height_method == 'max': grid_heights = df.groupby(['row', 'col'])['height'].max()
        elif self.map_height_method == 'min': grid_heights = df.groupby(['row', 'col'])['height'].min()
        else: grid_heights = df.groupby(['row', 'col'])['height'].mean()
        if grid_heights.empty: return None
        map_grid_points = []
        for (r, c), h in grid_heights.items():
            y_rel = (c + 0.5) * self.map_resolution - half_size; z_rel = (r + 0.5) * self.map_resolution - half_size
            y_world = y_rel + center_y; z_world = z_rel + center_z
            map_grid_points.append([h, y_world, z_world])
        if not map_grid_points: return None
        map_pcd_array = np.array(map_grid_points, dtype=np.float32)
        
        if self.lidar_centric_blinding_radius > 0 and self.estimated_ground_x is not None:
            distances_from_lidar = np.sqrt(map_pcd_array[:, 1]**2 + map_pcd_array[:, 2]**2)
            blinding_mask = distances_from_lidar <= self.lidar_centric_blinding_radius
            map_pcd_array[blinding_mask, 0] = self.estimated_ground_x

        publish_array = np.hstack([map_pcd_array, map_pcd_array[:, [0]]])
        map_msg = create_pointcloud2_msg(header, publish_array)
        self.elevation_map_pub.publish(map_msg)
        return map_pcd_array

def main(args=None):
    rclpy.init(args=args); node = LidarProcessor()
    try: rclpy.spin(node)
    except KeyboardInterrupt: node.get_logger().info("捕获到Ctrl+C，开始关闭...")
    finally: node.destroy_node(); rclpy.shutdown()

if __name__ == '__main__':
    main()